#pragma once
#include <iostream>
#include <Eigen/Dense>
#include "yaml-cpp/yaml.h"
#include <memory>
#include "continuous_linear_system.h"
#include "discrete_linear_system.h"
#include <fstream>
#include "common_tool/std_map_utility.h"

namespace auto_ros
{
namespace control
{
class VehicleDynBase : public ContinuousLinearSys
{
public:
	//init state fun when speed is 1
	VehicleDynBase(std::string dyn_yaml_path_name, uint state_dim, uint ctr_dim, uint distur_dim, uint out_dim)
		: ContinuousLinearSys(state_dim, ctr_dim, distur_dim, out_dim)
	{
		dyn_yaml_ = YAML::LoadFile(dyn_yaml_path_name);
		parm_ = dyn_yaml_.as<auto_ros::common_tool::double_map>();
		std::fstream fout(dyn_yaml_path_name + ".cor", std::ios::out);
		fout << dyn_yaml_;
		fout.close();
	}
	virtual void para_update_matrix(double speed)
	{
		std::cerr << "\033[31m para_update_matrix not impelement  \033[0m" << std::endl;
	}
	virtual void para_update_matrix(double speed_vx, double speed_vcg)
	{
		std::cerr << "\033[31m para_update_matrix not impelement  \033[0m" << std::endl;
	}
	virtual void para_update_matrix(auto_ros::common_tool::double_map &input)
	{
		std::cerr << "\033[31m para_update_matrix not impelement  \033[0m" << std::endl;
	}
	YAML::Node dyn_yaml_;
	auto_ros::common_tool::double_map parm_;
	auto_ros::common_tool::double_map log_map_;
};
} // namespace control
} // namespace auto_ros